// (c) Copyright 2013-2014 Stephen Gold <sgold@sonic.net>
// Distributed under the terms of the GNU General Public License

/*
 This file is part of the Bats Game.

 The Bats Game is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by the
 Free Software Foundation, either version 3 of the License, or (at your
 option) any later version.

 The Bats Game is distributed in the hope that it will be useful, but
 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 for more details.

 You should have received a copy of the GNU General Public License
 along with the Bats Game.  If not, see <http://www.gnu.org/licenses/>.
 */
package bats.unit;

import bats.Bats;
import bats.map.MapNode;
import bats.scene.SceneNode;
import com.jme3.animation.Bone;
import com.jme3.animation.Skeleton;
import com.jme3.bullet.PhysicsSpace;
import com.jme3.bullet.collision.shapes.CapsuleCollisionShape;
import com.jme3.bullet.collision.shapes.CollisionShape;
import com.jme3.bullet.collision.shapes.CompoundCollisionShape;
import com.jme3.bullet.collision.shapes.SphereCollisionShape;
import com.jme3.bullet.objects.PhysicsRigidBody;
import com.jme3.math.FastMath;
import com.jme3.math.Matrix3f;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import com.jme3.scene.Node;
import com.jme3.scene.Spatial;
import java.util.Map;
import java.util.TreeMap;
import java.util.logging.Level;
import java.util.logging.Logger;
import jme3utilities.MySkeleton;
import jme3utilities.MySpatial;
import jme3utilities.math.MyMath;
import jme3utilities.math.MyVolume;
import jme3utilities.math.VectorXZ;
import jme3utilities.physics.OverlapListener;
import jme3utilities.physics.SimpleSolidControl;

/**
 * A simple solid control to walk a bob around on the map until it becomes
 * impaired.
 * <p>
 * Uses dynamic feedback loops to keep each unimpaired bob (more or less)
 * upright and facing in the correct direction with its base supported. The
 * control is based on SimpleSolidControl instead of CharacterControl so that
 * the bob reacts naturally to collisions (provided its collision shape is not a
 * mesh).
 * <p>
 * Guidance uses the MoveControl interface to specify bob movement.
 * <p>
 * Each instance is disabled at creation.
 *
 * @author Stephen Gold <sgold@sonic.net>
 */
public class BobControl
        extends SimpleSolidControl
        implements MoveControl, OverlapListener {
    // *************************************************************************
    // constants

    /**
     * coefficient of friction for the physical object (&ge;0)
     */
    final private static float friction = 0.5f;
    /**
     * maximum turn rate magnitude (radians per second, &ge;0)
     */
    final private static float maxTurnRate = 2f;
    /**
     * maximum advance speed (in meters per sec, &ge;0) for a standard-sized bob
     */
    final private static float standardMaxAdvanceSpeed = 1.5f;
    /**
     * maximum back up speed (in meters per second, &ge;0) for a standard-sized
     * bob
     */
    final private static float standardMaxBackSpeed = 0.5f;
    /**
     * maximum sidestep speed (in meters per second, &ge;0) for a standard-sized
     * bob
     */
    final private static float standardMaxSideSpeed = 0.5f;
    /**
     * feedback gain to keep the bob supported (per second, &gt;0)
     */
    final private static float supportGain = 30f;
    /**
     * feedback gain to keep the bob facing the correct direction (per second)
     */
    final private static float turnGain = 15f;
    /**
     * message logger for this class
     */
    final private static Logger logger =
            Logger.getLogger(BobControl.class.getName());
    /**
     * model's cephalic or "up" direction (unit vector)
     */
    final private static Vector3f modelCephalicDirection = Vector3f.UNIT_Y;
    /**
     * feedback gain to keep the bob upright (per second)
     */
    final private static VectorXZ attitudeGain = new VectorXZ(0f, 30f);
    // *************************************************************************
    // fields
    /**
     * if true, the bob collapses; if false, the bob maintains an upright
     * attitude subject to kinematic animations
     */
    private boolean impaired = false;
    /**
     * number of destinations received since the previous physics tick (&ge;0)
     */
    private float destinationRequestCount = 0f;
    /**
     * number of guidance direction requests received since the previous physics
     * tick (&ge;0)
     */
    private float directionRequestCount = 0f;
    /**
     * map names of solid bones to information
     */
    final private Map<String, SolidBone> solidBones = new TreeMap<>();
    /**
     * which scene the bob is in: set by constructor
     */
    final private SceneNode scene;
    /**
     * which direction the bob intends to face (unit vector in map coordinates)
     */
    private VectorXZ directionGoal = null;
    /**
     * if destinationRequestCount is positive, the sum of all guidance
     * destinations received since the previous physics tick; otherwise, an
     * averaged destination from a prior physics tick or null if no guidance has
     * been received since newGuidanceInput()
     */
    private VectorXZ requestedDestination = null;
    /**
     * if directionRequestCount is positive, the sum of all guidance directions
     * received since the previous physics tick; otherwise, an averaged
     * direction from a prior physics tick or null if no guidance has been
     * received since newGuidanceInput()
     */
    private VectorXZ requestedDirection = null;
    // *************************************************************************
    // constructors

    /**
     * Instantiate a disabled control for an unimpaired bob.
     *
     * @param node which represents the bob (not null)
     * @param scene which contains the spatial (not null)
     * @param direction which the bob will initially face (not null)
     */
    public BobControl(Node node, SceneNode scene, VectorXZ direction) {
        super(false,
                scene.getWorld().getBobs().createShape(node),
                scene.getWorld().getBobs().calculateMass(node),
                scene.getWorld().getPhysics().getSpace());
        assert mass > 0f : mass;

        this.scene = scene;
        directionGoal = direction.clone(); // use spatial.getLocalRotation()?
        initializeSolidBones(node);

        assert spatial == null : spatial;
        assert !isEnabled();
        assert !isImpaired();
    }
    // *************************************************************************
    // new methods exposed

    /**
     * Add a torso to the bob's collision shape.
     *
     * @param result shape to augment (not null)
     */
    void addTorsoShape(CompoundCollisionShape result) {
        Bone spine = MySkeleton.getBone(spatial, "spine");
        Vector3f modelCenter = Bobs.getTorsoCenter();
        Quaternion orientation = spine.getModelSpaceRotation();
        Matrix3f modelRotation = orientation.toRotationMatrix();
        float modelScale = MySpatial.getUniformScale(spatial);

        int xAxis = 0;
        CapsuleCollisionShape abdomen = new CapsuleCollisionShape(
                0.1f * modelScale, 0.15f * modelScale, xAxis);
        Vector3f offset = MySkeleton.modelLocation(spine, 0f, 0.05f, 0.066f);
        offset.subtractLocal(modelCenter);
        offset.multLocal(modelScale);
        result.addChildShape(abdomen, offset, modelRotation);

        CapsuleCollisionShape chest = new CapsuleCollisionShape(
                0.12f * modelScale, 0.05f * modelScale, xAxis);
        offset = MySkeleton.modelLocation(spine, 0f, 0.27f, 0.04f);
        offset.subtractLocal(modelCenter);
        offset.multLocal(modelScale);
        result.addChildShape(chest, offset, modelRotation);

        float shoulderRadius = 0.05f * modelScale;

        SphereCollisionShape leftShoulder =
                new SphereCollisionShape(shoulderRadius);
        offset = MySkeleton.modelLocation(spine, 0.09f, 0.42f, 0f);
        offset.subtractLocal(modelCenter);
        offset.multLocal(modelScale);
        result.addChildShape(leftShoulder, offset, modelRotation);

        SphereCollisionShape rightShoulder =
                new SphereCollisionShape(shoulderRadius);
        offset = MySkeleton.modelLocation(spine, -0.09f, 0.42f, 0f);
        offset.subtractLocal(modelCenter);
        offset.multLocal(modelScale);
        result.addChildShape(rightShoulder, offset, modelRotation);
    }

    /**
     * Get the model coordinates of a bob's base.
     *
     * @return a new vector
     */
    Vector3f getBase() {
        Bone leftFoot = MySkeleton.getBone(spatial, "foot.L");
        Vector3f left = leftFoot.getModelSpacePosition();

        Bone rightFoot = MySkeleton.getBone(spatial, "foot.R");
        Vector3f right = rightFoot.getModelSpacePosition();
        /*
         * Decide whether weight is on the left foot (delta=-1)
         * or the right (delta=+1) or a mixture of the two.
         */
        float delta = MyMath.clamp((left.y - right.y) / 0.01f, 1f);
        float mix = (1f + delta) / 2f;
        Vector3f result = FastMath.interpolateLinear(mix, left, right);
        return result;
    }

    /**
     * Access the bone list.
     *
     * @return the pre-existing instance (not null)
     */
    Map<String, SolidBone> getSolidBones() {
        assert solidBones != null;
        return solidBones;
    }

    /**
     * Impair the unimpaired bob.
     */
    void impair() {
        assert !isImpaired();

        impaired = true;
        MySkeleton.setUserControl(spatial, true);
        /*
         * Calculate the average density of the old collision shape.
         */
        CollisionShape oldShape = getCollisionShape();
        float volume = MyVolume.volume(oldShape);
        float density = getMass() / volume; // kilograms per cubic world unit

        updateCollisionShape();
        /*
         * Recalculate mass of the object based on the torso only.
         */
        CollisionShape newShape = getCollisionShape();
        volume = MyVolume.volume(newShape);
        float newMass = volume * density;
        setMass(newMass);
        /*
         * Create a rigid body for each solid bone, starting with those which
         * have no solid parent.
         */
        Quaternion torsoInWorld = this.getPhysicsRotation();
        for (String name : solidBones.keySet()) {
            SolidBone solidBone = solidBones.get(name);
            if (solidBone.getSolidParent() == null) {
                solidBone.joinTo(this, density, torsoInWorld);
            }
        }
    }

    /**
     * Test whether the bob is impaired.
     *
     * @return true if impaired, false otherwise
     */
    final boolean isImpaired() {
        return impaired;
    }

    /**
     * Update the bob's collision shape.
     */
    void updateCollisionShape() {
        Bobs bobs = scene.getWorld().getBobs();
        CollisionShape shape = bobs.createShape(spatial);
        changeShape(shape);
    }
    // *************************************************************************
    // MoveControl methods

    /**
     * Get the direction in which the bob wants to face.
     *
     * @return a new unit vector
     */
    @Override
    public VectorXZ copyDirectionGoal() {
        return directionGoal.clone();
    }

    /**
     * Forget all past guidance.
     */
    @Override
    public void newGuidanceInput() {
        destinationRequestCount = 0;
        requestedDestination = null;
        directionRequestCount = 0;
        requestedDirection = null;
    }

    /**
     * Accept destination guidance.
     *
     * @param destination desired map location (not null)
     */
    @Override
    public void requestDestination(VectorXZ destination) {
        logger.log(Level.INFO, "destination={0}", destination);

        if (destinationRequestCount == 0f) {
            requestedDestination = new VectorXZ();
        }
        requestedDestination = requestedDestination.add(destination);
        destinationRequestCount++;
    }

    /**
     * Accept horizontal movement guidance relative to the bob.
     *
     * @param forward desired forward movement (in meters)
     * @param rightward desired rightward movement (in meters)
     */
    @Override
    public void requestMove(float forward, float rightward) {
        VectorXZ localOffset = new VectorXZ(forward, rightward);
        Bobs bobs = scene.getWorld().getBobs();
        VectorXZ direction = bobs.getMapPelvicDirection(spatial);
        VectorXZ mapOffset = localOffset.rotate(direction);
        requestOffset(mapOffset);
    }

    /**
     * Accept horizontal movement guidance relative to the bob.
     *
     * @param offsetMeters desired map offset (in meters, not null)
     */
    @Override
    public void requestOffset(VectorXZ offsetMeters) {
        assert offsetMeters != null;

        float scaleFactor = scene.getScaleFactor();
        VectorXZ offset = offsetMeters.divide(scaleFactor);
        VectorXZ mapLocation = MySpatial.getMapLocation(spatial);
        VectorXZ destination = mapLocation.add(offset);
        requestDestination(destination);
    }

    /**
     * Accept turning guidance.
     *
     * @param turn desired turn relative to the current direction goal (not
     * null)
     */
    @Override
    public void requestTurn(VectorXZ turn) {
        assert turn != null;

        if (directionRequestCount == 0f) {
            requestedDirection = new VectorXZ();
        }
        requestedDirection = requestedDestination.add(turn);
        directionRequestCount++;
    }
    // *************************************************************************
    // OverlapListener methods

    /**
     * React to an overlapping rigid body.
     *
     * @param overlappingBody the overlapping rigid body (not null)
     * @param overlappingSpatial the spatial of the overlapping rigid body (not
     * null)
     * @param localPoint the location of the overlap (rotated and translated to
     * this control's object, but at world scale)
     */
    @Override
    public void onOverlap(PhysicsRigidBody overlappingBody,
            Spatial overlappingSpatial, Vector3f localPoint) {
        assert overlappingBody != null;
        assert overlappingSpatial != null;
        assert localPoint != null;

        if (!isEnabled() || isImpaired()) {
            return;
        }
        float maxClearance =
                scene.getWorld().getBobs().getMaxClearance(spatial); // meters
        float baseY = getBase().y * MySpatial.getUniformScale(spatial);
        float clearanceNeeded = localPoint.y - baseY; // world units
        clearanceNeeded *= scene.getScaleFactor(); // convert to meters
        if (clearanceNeeded <= maxClearance) {
            /*
             * The collision point is on the bob's lower leg,
             * so raise its feet to increase ground clearance.
             */
            //TODO
        }
    }
    // *************************************************************************
    // RigidBodyControl methods

    /**
     * Callback to update this control. (Invoked once per frame.)
     *
     * @param elapsedTime since the previous update (in seconds, &ge;0)
     */
    @Override
    public void update(float elapsedTime) {
        super.update(elapsedTime);
        if (spatial == null || !isEnabled()) {
            return;
        }
        if (isImpaired()) {
            /*
             * Update joint angles from the ragdoll simulation.
             */
            Skeleton skeleton = MySkeleton.getSkeleton(spatial);
            Bone[] roots = skeleton.getRoots();
            for (int rootIndex = 0; rootIndex < roots.length; rootIndex++) {
                Bone rootBone = roots[rootIndex];
                updateImpaired(rootBone);
            }

        } else if (!Bats.isSimulationMode()) {
            /*
             * In pose mode, there are no physics ticks, so update the
             * collision shape here instead of in prePhysicsTick().
             */
            updateCollisionShape();
        }
    }
    // *************************************************************************
    // SimpleSolidControl methods

    /**
     * (Re-)Initialize this control each time it is added.
     */
    @Override
    public void onAdd() {
        super.onAdd();

        setFriction(friction);
        /*
         * Initialize the guidance inputs.
         * In the absence of guidance, the bob will stay put.
         */
        newGuidanceInput();
    }

    /**
     * Callback invoked before each physics tick to set linear and angular rates
     * and update the collision shape.
     *
     * @param physicsSpace
     * @param simInterval (in seconds, &gt;0)
     */
    @Override
    public void prePhysicsTick(PhysicsSpace physicsSpace, float simInterval) {
        super.prePhysicsTick(physicsSpace, simInterval);
        if (spatial == null || !isEnabled() || !scene.isEnabled()
                || isImpaired()) {
            return;
        }
        assert simInterval > 0f : simInterval;
        /*
         * If there have been multiple guidance inputs since the previous tick,
         * average them together.
         */
        if (directionRequestCount > 0f) {
            requestedDirection = requestedDestination.normalize();
            directionRequestCount = 0f;
        }
        if (destinationRequestCount > 1f) {
            requestedDestination =
                    requestedDestination.divide(destinationRequestCount);
            destinationRequestCount = 0f;
        }
        /*
         * Turn the bob per guidance.
         */
        if (requestedDirection != null) {
            float maxTurnAngle = maxTurnRate * simInterval;
            VectorXZ turnDirection = requestedDirection.clone();
            turnDirection = turnDirection.clampDirection(maxTurnAngle);
            directionGoal = directionGoal.rotate(turnDirection);
        }
        /*
         * Use feedback to maintain an upright attitude.
         */
        Matrix3f orientation = getPhysicsRotationMatrix();
        Vector3f cephalicDirection = orientation.mult(modelCephalicDirection);
        VectorXZ attitude = new VectorXZ(cephalicDirection);
        /* feedback seeks to zero out this attitide vector */
        VectorXZ attitudeControlSignal = attitude.rotate(attitudeGain);
        /*
         * Use feedback to face the correct direction.
         */
        Bobs bobs = scene.getWorld().getBobs();
        VectorXZ direction = bobs.getMapPelvicDirection(spatial);
        float directionError = direction.directionError(directionGoal);
        float turnControlSignal = turnGain * directionError; // clockwise=+
        /*
         * Combine the two attitude control signals.
         */
        attitudeControlSignal.rotate(direction);
        Vector3f angularRates =
                attitudeControlSignal.toVector3f(-turnControlSignal);
        setAngularVelocity(angularRates);
        /*
         * Horizontal control signal: move toward the desired location.
         */
        VectorXZ localVelocity;
        if (requestedDestination == null) {
            localVelocity = new VectorXZ();
        } else {
            VectorXZ mapLocation = MySpatial.getMapLocation(spatial);
            VectorXZ mapOffset = requestedDestination.subtract(mapLocation);
            VectorXZ move = mapOffset.rotate(direction.mirrorZ());
            VectorXZ localVelocityGoal = move.divide(simInterval);
            localVelocity = restrictMove(localVelocityGoal);
            float speed = localVelocity.length(); // world units per second
            if (speed > 0f) {
                float distance = move.length(); // world units
                float estimatedTravelTime = distance / speed;
                if (estimatedTravelTime < simInterval) {
                    /*
                     * Reduce speed to avoid overshooting the destination.
                     */
                    localVelocity = localVelocity.mult(estimatedTravelTime
                            / simInterval);
                }
            }
        }
        VectorXZ mapVelocity = localVelocity.rotate(directionGoal);
        /*
         * Vertical control signal: keep at least one of
         * the bob's feet supported.
         */
        float startHeight = bobs.getMaxClearance(spatial) + 0.01f; // meters
        MapNode map = scene.getWorld().getMap();
        float verticalVelocity = 0f; // meters per second

        Vector3f leftFoot = MySkeleton.worldLocation(spatial, "foot.L");
        Float leftAboveSupport = map.measureDrop(leftFoot, startHeight);
        if (leftAboveSupport != null) {
            verticalVelocity = -supportGain * leftAboveSupport;
        }
        Vector3f rightFoot = MySkeleton.worldLocation(spatial, "foot.R");
        Float rightAboveSupport = map.measureDrop(rightFoot, startHeight);
        if (rightAboveSupport != null) {
            float signal = -supportGain * rightAboveSupport;
            if (signal > verticalVelocity) {
                verticalVelocity = signal;
            }
        }
        float scaleFactor = scene.getScaleFactor();
        verticalVelocity /= scaleFactor;
        /*
         * Combine the two translation control signals.
         */
        Vector3f linearVelocity = mapVelocity.toVector3f(verticalVelocity);
        setLinearVelocity(linearVelocity);

        updateCollisionShape();
    }
    // *************************************************************************
    // private methods

    /**
     * Add a solid bone to the bob.
     *
     * @param model node which represents the bob (not null)
     * @param boneName name of the bone (not null)
     * @param headRadius (in model units, &ge;0)
     * @param shaftRadius (in model units, &ge;0)
     */
    private void addSolidBone(Node model, String boneName, float headRadius,
            float shaftRadius) {
        assert model != null;
        assert boneName != null;
        assert headRadius >= 0f : headRadius;
        assert shaftRadius >= 0f : shaftRadius;

        SolidBone solidBone = new SolidBone(headRadius, shaftRadius, solidBones,
                model, boneName);
        solidBones.put(boneName, solidBone);
    }

    /**
     * Add a pair of bilaterally symmetric solid bones to the bob.
     *
     * @param model node which represents the bob, in bind pose (not null)
     * @param namePrefix of the bones (not null)
     * @param headRadius (in model units, &ge;0)
     * @param shaftRadius (in model units, &ge;0)
     */
    private void addSolidBonePair(Node model, String namePrefix,
            float headRadius, float shaftRadius) {
        assert model != null;
        assert namePrefix != null;
        assert headRadius >= 0f : headRadius;
        assert shaftRadius >= 0f : shaftRadius;

        addSolidBone(model, namePrefix + ".L", headRadius, shaftRadius);
        addSolidBone(model, namePrefix + ".R", headRadius, shaftRadius);
    }

    /**
     * Initialize the list of solid bones for a bob.
     *
     * @param model node which represents the bob (not null)
     */
    private void initializeSolidBones(Node model) {
        assert model != null;

        addSolidBone(model, "neck", 0.08f, 0.07f);
        /*
         * two arms
         */
        addSolidBonePair(model, "uparm", 0.03f, 0.036f);
        addSolidBonePair(model, "farm", 0.03f, 0.03f);
        addSolidBonePair(model, "wrist", 0.03f, 0.018f);
        /*
         * two legs
         */
        addSolidBonePair(model, "thigh", 0.05f, 0.06f);
        addSolidBonePair(model, "shin", 0.03f, 0.03f);
        addSolidBonePair(model, "foot", 0.008f, 0.01f);
    }

    /**
     * Restrict movement speed in the horizontal plane where +X=forward and
     * +Z=right. Interpolation between the four principal directions is
     * performed using two ellipses, one for the +X half-plane and another for
     * the -X half-plane.
     *
     * @param request (in world units per second, not null, unaffected)
     */
    private VectorXZ restrictMove(VectorXZ request) {
        Bobs bobs = scene.getWorld().getBobs();
        float relativeSize = bobs.getRelativeSize(spatial);
        float scaleFactor = scene.getScaleFactor();
        float maxAdvance = standardMaxAdvanceSpeed * relativeSize / scaleFactor;
        float maxBack = standardMaxBackSpeed * relativeSize / scaleFactor;
        float maxSide = standardMaxSideSpeed * relativeSize / scaleFactor;
        if (request.getX() > 0f) {
            /*
             * more forward than backward
             */
            return request.clampElliptical(maxAdvance, maxSide);
        } else {
            return request.clampElliptical(maxBack, maxSide);
        }
    }

    /**
     * Update a bone and its descendants while the bob is impaired. NOTE:
     * recursive!
     *
     * @param bone (not null)
     */
    private void updateImpaired(Bone bone) {
        String name = bone.getName();
        SolidBone solid = solidBones.get(name);
        if (solid == null) {
            bone.updateWorldVectors();
        } else {
            Vector3f location = solid.physicsTail();
            Quaternion rotation = solid.physicsOrientation();
            MySkeleton.positionInWorld(spatial, bone, location, rotation);
        }

        for (Bone child : bone.getChildren()) {
            updateImpaired(child);
        }
    }
}